/**
 * \file main.c
 * Main code
 * 
 * Main file for USB Mass storage device.
 * 
 * AT91SAM7S-128 USB Mass Storage Device with SD Card by Michael Wolf\n
 * Copyright (C) 2008 Michael Wolf\n\n
 * 
 * This program is free software: you can redistribute it and/or modify\n
 * it under the terms of the GNU General Public License as published by\n
 * the Free Software Foundation, either version 3 of the License, or\n
 * any later version.\n\n
 * 
 * This program is distributed in the hope that it will be useful,\n
 * but WITHOUT ANY WARRANTY; without even the implied warranty of\n
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n
 * GNU General Public License for more details.\n\n
 * 
 * You should have received a copy of the GNU General Public License\n
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.\n
 *
 */
 
/**
 * 
 * \mainpage AT91SAM7S128 USB Mass Storage Device with SD card
 * 
 * \version 0.10b
 * 
 * \section intro_sec Introduction
 *
 * This documentation is generated by Doxygen and documents a example source code
 * to implement an USB mass storage device on Atmel AT91SAM7S-128 using a SD card as
 * storage medium.\n\n
 *
 * Now with support for SDHC cards. Tested with Sandisk 4GB card.\n\n
 * 
 * The code was taken from another project and therefore requires some basic hardware
 * to run.\n
 * Especially the code related to the power button needs to be changed for your requirements.\n
 * \see Schematic for hardware details.\n\n
 * All USB functions are based on Atmels USB MSD framework. But this code has been modified and
 * reduced to a minimum requirment to save resources and make the code more readable.\n
 * Transfer rates are not very fast, bottleneck here is the bulk endpoint size of only 
 * 64 byte.\n\n
 * 
 * Code uses 23108 bytes program memory and 4312 bytes RAM.\n\n
 * 
 * Debugging output for all parts of the code can be configured in file trace.h \n
 * Using the debugging unit with 115200 Baud 8N1
 *  
 * These documentation pages are generated regularly from the currect source code.
 * 
 * For questions, bug reports or problems about contact me, Michael, at <a href="mailto:michael@mictronics.de">michael@mictronics.de</a>
 *
 * \author  Michael Wolf
 * \see http://www.mictronics.de
 * 
 * \image html SAM7_USB_MSD_example_schematic.png "Example Schematic"
 * 
 * AT91SAM7S-128 USB Mass Storage Device with SD Card by Michael Wolf\n
 * Copyright (C) 2008 Michael Wolf\n\n
 *
 * This program is free software: you can redistribute it and/or modify\n
 * it under the terms of the GNU General Public License as published by\n
 * the Free Software Foundation, either version 3 of the License, or\n
 * any later version.\n\n
 * 
 * This program is distributed in the hope that it will be useful,\n
 * but WITHOUT ANY WARRANTY; without even the implied warranty of\n
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n
 * GNU General Public License for more details.\n\n
 * 
 * You should have received a copy of the GNU General Public License\n
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.\n
 * 
 */ 
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include "hardware_conf.h"
#include "firmware_conf.h"
#include "board_lowlevel.h"
#include <utils/macros.h>

#include <screen/screen.h>
#include <screen/font.h>
#include <debug/trace.h>
#include <utils/interrupt_utils.h>
#include <utils/time.h>
#include <peripherals/serial.h>
#include <utils/delay.h>
#include <peripherals/pmc/pmc.h>
#include <peripherals/spi.h>
#include <peripherals/i2c.h>
#include <peripherals/isl6295.h>
#include <utils/rprintf.h>
#include <sdcard/sdcard.h>
#include <fatfs/ff.h>
#include <fat/fat.h>
#include <usb/usb_msd.h>
#include <usb/usb_sbc.h>
#include <usb/usb_lun.h>
#include <usb/usb_bot.h>
#include <usb/usb_dsc.h>
#include <usb/usb_std.h>
#include <usb/usb_drv.h>
#include "blconfgui.h"
#include "nvmrc.h"
#include "main.h"

volatile AT91PS_PIO  pPIOA = AT91C_BASE_PIOA;     // PIO controller
volatile AT91PS_PIO  pPIOB = AT91C_BASE_PIOB;     // PIO controller

//scr_buf_t sram_screen[10];
scr_buf_t * scrbuf=NULL;
//scr_bitmapbuf_t * bitmap[(20*20)];

uint32_t gen_buffer[512];   //!< Buffer for general use
uint8_t rbuf[512];    //!< Buffer for general use

volatile int (*extcall)();
volatile uint8_t *extmem;

int chargerManager(void);
void chargerStat(void);
void volt_meas(void);

FATFS  imgfatfs; 
FILINFO imagef_info;
FIL imagef;

/**
 * The main function.
 * 
 * \return Zero
 *
 */
int main (void) {

    volatile AT91PS_UDP  pUDP = AT91C_BASE_UDP;
    volatile AT91PS_AIC  pAIC=AT91C_BASE_AIC;
    FRESULT r;
    UINT readbytes;
    uint32_t image_size,i;

#if 0   // set to 1 to enable the watchdog timer
    // set watchdog timeout to 2 seconds
    *AT91C_WDTC_WDMR = AT91C_WDTC_WDD | AT91C_WDTC_WDRSTEN | AT91C_WDTC_WDDBGHLT | 0x200;
    // kick the dog
    *AT91C_WDTC_WDCR = 0xA5000000 | AT91C_WDTC_WDRSTT;
#else
    #warning Watchdog is disabled!
    /**
     * \warning Watchdog timer disabled!
     */
    // disable the watchdog
    *AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS;
#endif

    pioInit();
    
    delayms(200);
    init_lowlevel_io(); // init all IO pins and ports

    timer_init();   // init timer for timeout functions
    


    dbg_usart_init();  // init DBG USART
    TRACE_ALL("Powering up...\n\r");



    spi_init(); // init all SPI hardware and channels
    TRACE_ALL("Spi init ok.\n\r");
    
    //?charger fix? TODO $$$$$  AVR se sekne pri low bat a reboot
    //pPIOA->PIO_CODR = CHARGEEN_PIN; //set to log0 
    
        
    //screen
    (void) scrInit();
    delayms(200);
    //read config from NVram:
    readSetupConf();
    scrrot = blconf.screenRotate;

    fontSetCharPos(0,0);
    fontColor = SCR_COLOR_YELLOW;
    TRACE_SCR("Dynawa.TCH1 bl 0.3a\n\r");
    fontColor = SCR_COLOR_RED;
    fontSetCharPos(5,119);
    TRACE_SCR("Press to enter SETUP>>>\n\r");
    //temp. enable charge:
    fontSetCharPos(0,8);
    //charger management
    //while (1) { volt_meas(); delayms(500); }
    chargerManager();
    
    delayms(1500);
    //==========SETUP ENTRY POINT=============================
    if ((!((pPIOB->PIO_PDSR)&(BUT4_MASK)))||(!((pPIOB->PIO_PDSR)&(BUT0_MASK)))||(!((pPIOB->PIO_PDSR)&(BUT2_MASK)))||(!((pPIOB->PIO_PDSR)&(BUT3_MASK))))
    {
       if( sd_init() != SD_OK )
       {
    	   TRACE_SCR("SD card init failed, skipping!\n\r");
    	   delayms(2000);
    	 }
       blConfig();
    }
    
    //
    fontColor = SCR_COLOR_WHITE;
    TRACE_SCR("Processing POST.\n\r");
    if (blconf.POST_sram) { TRACE_SCR("PSRAM 64Mb    [");fontColor = SCR_COLOR_GREEN;TRACE_SCR("OK");fontColor = SCR_COLOR_WHITE;TRACE_SCR("]\n\r"); }
    if (blconf.POST_rtc) { TRACE_SCR("RTC           [");fontColor = SCR_COLOR_GREEN;TRACE_SCR("OK");fontColor = SCR_COLOR_WHITE;TRACE_SCR("]\n\r"); }
    if (blconf.POST_gasgauge) { TRACE_SCR("Battery&gas   [");fontColor = SCR_COLOR_GREEN;TRACE_SCR("OK");fontColor = SCR_COLOR_WHITE;TRACE_SCR("]\n\r"); }
    if (blconf.POST_accel) { TRACE_SCR("Accelerometer [");fontColor = SCR_COLOR_GREEN;TRACE_SCR("OK");fontColor = SCR_COLOR_WHITE;TRACE_SCR("]\n\r"); }
    if (blconf.POST_led) {TRACE_SCR("LED driver    [");fontColor = SCR_COLOR_GREEN;TRACE_SCR("OK");fontColor = SCR_COLOR_WHITE;TRACE_SCR("]\n\r"); }

    
    TRACE_SCR("SD card       [");    
    // init SD card interface
    if( sd_init() != SD_OK )
    {
    	TRACE("SD card init failed!\nPowering off...\n");
    	fontColor = SCR_COLOR_RED;    	
      TRACE_SCR("ERR");
      fontColor = SCR_COLOR_WHITE;TRACE_SCR("]\n\r");
      TRACE_SCR("System Halted.\n\r");
      TRACE_SCR("No USB massstorage.\n\r");
      TRACE_SCR("Unable to run image.");
      while (1)
      {
        if (tick1s>5) { tick1s =0; chargerStat(); }
      }                              
    }
    
    TRACE_ALL("SD init ok.\n\n");
    fontColor = SCR_COLOR_GREEN;    	
    TRACE_SCR("OK");
    fontColor = SCR_COLOR_WHITE;TRACE_SCR("]\n\r");    
    
    //TODO replace with fuction from fatfs!   
    fat_init();
       
    if ((r = f_mount (0, &imgfatfs)) == FR_OK)
    {
       
       //while(1);
       
    //fat_init(); // init file system
    
     
    
    delay(500);
    //logic ^^ xor? C stupidity ?
    if ((((pPIOB->PIO_PDSR)&(BUT1_MASK))==0)^(blconf.defMSDBoot==0))
    {
      //fimage = fat_open("main.bin",_O_RDONLY);
      readBootImage();
      if (f_stat(bootname,&imagef_info)!=FR_OK)
      {
        TRACE_SCR("unable get file size\n\r");
      } else {
        TRACE_SCR("%s:%dkB\n\r",bootname,imagef_info.fsize/1024);
      }
      
      if ((r=f_open(&imagef, bootname, FA_READ))== FR_OK)
      {                
        image_size = imagef_info.fsize;
        
        //load image to mem and launch:
        extmem = 0x10000000;
        TRACE_ALL("image read opened.\n\r");
        //read SD image
        //while ((readbytes=fat_read(fimage, rbuf, 512)))
        do
        { 
          if ((r=f_read(&imagef,rbuf,512,&readbytes))!= FR_OK)
          {
            TRACE_SCR("image read failure.\n\r");
            TRACE_ALL("image read failure.\n\r");
            while (1);
          }     
          for (i=0;i<readbytes;i++)
    			{
            *extmem = rbuf[i];                    
            extmem++;                  
          }
          
        } while (readbytes>=512);
        
        f_close(&imagef);
        TRACE_SCR("LOADED.%x\n\r",extmem);
        //disable interrupts:            
        extmem = 0x10000000;            
        //extmem[0]=0xaa;
        TRACE_ALL("[%x][%x][%x][%x][%x][%x]",*(extmem++),*(extmem++),*(extmem++),*(extmem++),*(extmem++),*(extmem++));
        pAIC->AIC_IDCR = 0xffffffff;
        
        //close:
        
        extcall=0x10000000;
        i=extcall();
        TRACE_ALL("?err");
        while (1);
        
      } else {
        TRACE_SCR("main.bin not found\n\r");      
      }
    } 
    } else {
      TRACE_ALL("FAT init err.\n\r");
      TRACE_SCR("FAT init err.\n\r");
    }   
    TRACE_SCR("run USB massStorage\n\r");
    //(void) fat_read(fimage, rbuf, 50);
    //rbuf[55] = 0;//zeroterm
    //TRACE_ALL(rbuf);     
    
    
    pUDP->UDP_TXVC |= AT91C_UDP_PUON;
        
    delayms(20);
    /*
    while (1)
    {
      delayms(500);
      SET(pPIOA->PIO_SODR, PIN_LED);
      delayms(500);
      TRACE("tick...\n\r");
      SET(pPIOA->PIO_CODR, PIN_LED);      
     } 
      */        
    tick1s=0;
    // start and run state maschine
    while(1)
    {
        if (tick1s>5) { tick1s =0; chargerStat(); }
        // kick the dog
        *AT91C_WDTC_WDCR = 0xA5000000 | AT91C_WDTC_WDRSTT;
        
        // check for USB connection and run handler if necesarry
        if ( usb_check_bus_status() && ISCLEARED(usb_device_state,USB_STATE_SUSPENDED) )
        {
        
            if (tick1s>5) { tick1s =0; chargerStat(); }
            
            // LUNs initialization
            lun_init(( unsigned char *)&gen_buffer, sd_info(), bytes_per_sector);
            
            // Bulk only driver initialization
            bot_init(1);

            // Wait for the device to be configured
            while (ISCLEARED(usb_device_state,USB_STATE_CONFIGURED));
            
            /*
             * Run state machine for Bulk only transfer as long
             * as we are connected to USB and device is configured.
             * USB standard traffic is handled by interrupts only
             */
            TRACE_SCR("USB MSD connected.\n\r");             
            while( ISCLEARED(pPIOB->PIO_PDSR, PIN_USB_DETECT)
                   && ISSET(usb_device_state, USB_STATE_CONFIGURED)
                 )
            {
                // kick the dog
                *AT91C_WDTC_WDCR = 0xA5000000 | AT91C_WDTC_WDRSTT;
                // Run bulk only transfer
                bot_state_machine();

                if( ISSET(usb_device_state,USB_STATE_SUSPENDED) )
                {
                	// End USB MSD on save removal of device.
                	TRACE("USB Suspended.\nPowering off...\n");
                	delayms(100);
                	  ///?????
                    //pPIOA->PIO_PER   = PIN_POWER_ON; // Enable PIO pin
                    //pPIOA->PIO_PPUDR = PIN_POWER_ON; // pullup
                    //pPIOA->PIO_ODR   = PIN_POWER_ON; // Enable input
                    while(1) if (tick1s>5) { tick1s =0; chargerStat(); };
                }
                
                if (tick1s>3) { tick1s =0; chargerStat(); }
                /** \todo Add some code to handle removal of SD card while USB is connected */
            }
        }
    }   // end while(1) main loop
       
    return 0;   // we should never reach this return
}   // end of int main(void)


/**
 * IO initialization
 * 
 * Initialization for IO pins and ports and internal hardware.
 * 
 */
void init_lowlevel_io(void)
{
    volatile AT91PS_SMC2	pSMC = AT91C_BASE_SMC;
    volatile AT91PS_EBI  pEBI = AT91C_BASE_EBI;
    volatile AT91PS_UDP  pUDP = AT91C_BASE_UDP;
    volatile AT91PS_PMC	pPMC = AT91C_BASE_PMC;
    
    // enable reset-button
    //*AT91C_RSTC_RMR = ( 0xA5000000 | AT91C_RSTC_URSTEN );
    
    *AT91C_PMC_PCER =   (1 << AT91C_ID_PIOA) |  // Enable Clock for PIO
                        (1 << AT91C_ID_IRQ0);  // Enable Clock for IRQ0

#if 1
    // set the POWER_ON I/O bit 
    SET(pPIOA->PIO_PER, PIN_POWER_ON);  // Enable PIO pin
    SET(pPIOA->PIO_OER, PIN_POWER_ON);  // Enable output
    SET(pPIOA->PIO_SODR, PIN_POWER_ON);  // Set Low to enable power
    
    SET(pPIOA->PIO_PER, PIN_LED);  // Enable PIO pin
    SET(pPIOA->PIO_OER, PIN_LED);  // Enable output
    SET(pPIOA->PIO_SODR, PIN_LED);  // Set Low to enable power
#else
    #warning Enable Power Pin!
#endif


    // disable USB pull-up 
    //SET(pPIO->PIO_PER, PIN_USB_PULLUP);   // Enable PIO pin
    //SET(pPIO->PIO_OER, PIN_USB_PULLUP);   // Enable outputs
    //SET(pPIO->PIO_SODR, PIN_USB_PULLUP);  // Set high

    // configure USB detection pin
    SET(pPIOB->PIO_PER, PIN_USB_DETECT);     // enable PIO pin
    SET(pPIOB->PIO_ODR, PIN_USB_DETECT);     // set pin as input
    SET(pPIOB->PIO_PPUER, PIN_USB_DETECT);   // enable pull-up
    
    SET(pPIOA->PIO_PER, PIN_CHARGING);     // enable PIO pin
    SET(pPIOA->PIO_ODR, PIN_CHARGING);     // set pin as input
    SET(pPIOA->PIO_PPUER, PIN_CHARGING);   // enable pull-up

   SET(pPIOA->PIO_PER, PIN_CHARGEDONE);     // enable PIO pin
    SET(pPIOA->PIO_ODR, PIN_CHARGEDONE);     // set pin as input
    SET(pPIOA->PIO_PPUER, PIN_CHARGEDONE);   // enable pull-up
    // Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock
    // Required to write to UDP_TXVC
    SET(pPMC->PMC_SCER, AT91C_PMC_UDP);
    SET(pPMC->PMC_PCER, (1 << AT91C_ID_UDP));
    
    //i2c
    SET(pPMC->PMC_PCER, (1 << AT91C_ID_TWI));
    
    SET(pUDP->UDP_TXVC, AT91C_UDP_TXVDIS);  // disable UDP tranceiver
                                            // because enabled by default after reset
    
    // Disables the 48MHz USB clock UDPCK and System Peripheral USB Clock
    // Save power
    //SET(pPMC->PMC_SCDR, AT91C_PMC_UDP);
    //SET(pPMC->PMC_PCDR, (1 << AT91C_ID_UDP));
    
    //extmem
    
    pEBI->EBI_CSA = 0x0;    
    //pSMC->SMC2_CSR[0] = 0x0000B081;
    pSMC->SMC2_CSR[0] = 0x0000B082;
    
    //enable buttons readout
    pPMC->PMC_PCER = (1<<AT91C_ID_PIOB);
    
}   // end of void init_lowlevel_io(void)

void volt_meas(void)
{
  // divider 0,18032786885245901639344262295082
  // ref: 3.3V
  // k= 17,87109375
  //volatile AT91PS_PIO  pPIO = AT91C_BASE_PIOA; 
  //volatile AT91PS_TC pTC = AT91C_BASE_TC0;
  volatile AT91PS_ADC pADC = AT91C_BASE_ADC;
  uint32_t result;
  static uint32_t filt;
  static char buf[32];
  
  pADC->ADC_CR = 0x1;//rst
  pADC->ADC_MR =  0x0f1f3f10; //mck/30
  pADC->ADC_CHDR=0xffffffff;
  //pADC->ADC_CHER= AT91C_ADC_CH5;
  //pADC->ADC_CHER= AT91C_ADC_CH4;//usb sense
  pADC->ADC_CHER= AT91C_ADC_CH7;//current sense
  
  pADC->ADC_IDR = 0xffffffff;
  
  
    pADC->ADC_CR = 0x2;//start conversion
    
    while (!(pADC->ADC_SR&AT91C_ADC_DRDY)) asm volatile ("nop");
    
    result = (pADC->ADC_LCDR);
    result = (result*8);
    filt = (7*result + filt)/8;
    
    
    //srprintf(buf,"Volt:%d ",filt);
    fontSetCharPos(0,8);
    TRACE_SCR("%d  ",filt);
        
  
}  

int chargerManager(void)
{
  
  volatile AT91PS_PIO	pPIOA = AT91C_BASE_PIOA;	
  int32_t packVoltage;
  uint16_t t;
  uint8_t b;
  //measure pack voltage:
  i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_OPmode<<10)|(I2CGG_REG_OPmode), I2CMASTER_WRITE);
	i2cWriteByte(1);
	i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_OPmode<<10)|(I2CGG_REG_OPmode), I2CMASTER_WRITE);
	i2cWriteByte(2);
	i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_OPmode<<10)|(I2CGG_REG_OPmode), I2CMASTER_WRITE);
	i2cWriteByte(0);
	
  i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_ADconfig<<10)|(I2CGG_REG_ADconfig), I2CMASTER_WRITE);
	i2cWriteByte(0x80);
	
	
	//i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_ITctrl<<10)|(I2CGG_REG_ITctrl), I2CMASTER_WRITE);
	//i2cWriteByte(I2CGG_DEFVAL_ITctrl);
	
  i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_VPctrl<<10)|(I2CGG_REG_VPctrl), I2CMASTER_WRITE);
	i2cWriteByte(I2CGG_DEFVAL_VPctrl);
	
	//i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_VPctrl<<10)|(I2CGG_REG_VPctrl), I2CMASTER_READ);
	//b=i2cReadByte();
	
  //TRACE_ALL("gas gauge ADC Vpack ON:VPctrl=%x\n\r",b); 
  delayms(300);
  
    i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_VPres<<10)|(I2CGG_REG_VPres), I2CMASTER_READ);
    b=i2cReadByte();//TRACE_ALL("[%x]",b);
    t = (uint16_t)b;
    i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_VPres<<10)|(I2CGG_REG_VPres+1), I2CMASTER_READ);
    b=i2cReadByte();//TRACE_ALL("[%x]",b);
    t |= (((uint16_t)b)<<8);
    
    packVoltage = ((t>>6)*199)/10;
    TRACE_ALL("battery read:%x hex\n\r", t);
    TRACE_ALL("battery voltage:%d mV\n\r", packVoltage); 
    TRACE_SCR("bat:%dmV,", packVoltage); 
    //delayms(200);
    if (packVoltage<4050) { pPIOA->PIO_CODR = CHARGEEN_PIN; //set to log0  
                            pPIOA->PIO_SODR = USBPEN2_PIN;  
                                                    
                            TRACE_SCR("Enabled,");
                          } else {
                            pPIOA->PIO_SODR = CHARGEEN_PIN;
                            pPIOA->PIO_SODR = USBPEN2_PIN;  
                            TRACE_SCR("Disabled,");
                          }  
    delay(200);
    if (ISCLEARED(pPIOA->PIO_PDSR, PIN_CHARGING))
    {
      TRACE_SCR("ON");
    } else 
    {
      TRACE_SCR("OFF");
    }             
                          
    TRACE_SCR("\n\r");                      
  //}
  //current meas
	i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_Ictrl<<10)|(I2CGG_REG_Ictrl), I2CMASTER_WRITE);
	i2cWriteByte(I2CGG_DEFVAL_Ictrl);
  
  
  return 0;
} 

void chargerStat(void)
{
   
   int32_t packVoltage, packCurrent;
  uint16_t t;
  uint8_t b;
   static uint8_t chargedone=0;
   static uint8_t ypos=111;
  
   i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_VPres<<10)|(I2CGG_REG_VPres), I2CMASTER_READ);
    b=i2cReadByte();//TRACE_ALL("[%x]",b);
    t = (uint16_t)b;
    i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_VPres<<10)|(I2CGG_REG_VPres+1), I2CMASTER_READ);
    b=i2cReadByte();//TRACE_ALL("[%x]",b);
    t |= (((uint16_t)b)<<8);
    
    packVoltage = ((t>>6)*199)/10;
    
    i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_Ires<<10)|(I2CGG_REG_Ires), I2CMASTER_READ);
    b=i2cReadByte();//TRACE_ALL("[%x]",b);
    t = (uint16_t)b;
    i2cMasterConf(I2CGG_PHY_ADDR, 2, (I2CGG_BANK_Ires<<10)|(I2CGG_REG_Ires+1), I2CMASTER_READ);
    b=i2cReadByte();//TRACE_ALL("[%x]",b);
    t |= (((uint16_t)b)<<8);
    
    if (t&0x8000)
      packCurrent = -(((int32_t)(t&0x7fff)*157)/10000);//mAmps
    else
      packCurrent = ((int32_t)(t&0x7fff)*157)/10000;//mAmps  
    
    scrWriteRect(0,0,159,127,0);
    fontSetCharPos(0,ypos);
    
    fontColor = SCR_COLOR_RED;
    TRACE_SCR("Charger Stat: ");
    if (ISCLEARED(pPIOA->PIO_PDSR, PIN_CHARGING))
    {
      TRACE_SCR("Charging.\n\r");
    } else {
      if (ISCLEARED(pPIOA->PIO_PDSR, PIN_CHARGEDONE))
      {
        chargedone=1;
        pPIOA->PIO_SODR = CHARGEEN_PIN; //disable
      } else {
    
        TRACE_SCR("No charge.\n\r");
      }
    }
    if (chargedone) TRACE_SCR("DONE.\n\r");
    fontColor = SCR_COLOR_WHITE;
    TRACE_SCR("bat:%dmV, curr%dmA  ", packVoltage, packCurrent); 
    ypos +=4;
    if (ypos>111) ypos=0;
    
}